{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center><img src=\"../image/CLBLOGO.jpg\" alt=\"创乐博\" style=\"width: 300px;\"/></center>\n",
    "\n",
    "# 11.OpenCV 摄像头云台人脸追踪\n",
    "\n",
    "@－－－－湖南创乐博智能科技有限公司－－－－<br>\n",
    "@  文件名：11.OpenCV摄像头云台人脸追踪.ipynb <br>\n",
    "@  版本：V2.0 <br>\n",
    "@  author: zhulin<br>\n",
    "@  说明：OpenCV摄像头云台人脸追踪<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 为了可以打开摄像头，这里关闭摄像头所有进程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入必要的库\n",
    "import cv2\n",
    "import numpy as np\n",
    "from LOBOROBOT import LOBOROBOT # 载入机器人库\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "clbrobot = LOBOROBOT()  # 实例化机器人对象\n",
    "# Configure min and max servo pulse lengths\n",
    "servo_min = 150  # Min pulse length out of 4096\n",
    "servo_max = 600  # Max pulse length out of 4096"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 线程函数操作库\n",
    "import threading # 线程\n",
    "import ctypes\n",
    "import inspect\n",
    "\n",
    "# 线程结束代码\n",
    "def _async_raise(tid, exctype):\n",
    "    tid = ctypes.c_long(tid)\n",
    "    if not inspect.isclass(exctype):\n",
    "        exctype = type(exctype)\n",
    "    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))\n",
    "    if res == 0:\n",
    "        raise ValueError(\"invalid thread id\")\n",
    "    elif res != 1:\n",
    "        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)\n",
    "        raise SystemError(\"PyThreadState_SetAsyncExc failed\")\n",
    "        \n",
    "def stop_thread(thread):\n",
    "    _async_raise(thread.ident, SystemExit)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "def bgr8_to_jpeg(value, quality=75):\n",
    "    return bytes(cv2.imencode('.jpg', value)[1])\n",
    "\n",
    "frame_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "display(frame_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import libcamera\n",
    "from picamera2 import Picamera2\n",
    "\n",
    "picamera = Picamera2()\n",
    "config = picamera.create_preview_configuration(main={\"format\": 'RGB888', \"size\": (320, 240)},\n",
    "                                               raw={\"format\": \"SRGGB12\", \"size\": (1920, 1080)})\n",
    "config[\"transform\"] = libcamera.Transform(hflip=0, vflip=0)\n",
    "picamera.configure(config)\n",
    "picamera.start()\n",
    "\n",
    "dispW=320\n",
    "dispH=240\n",
    "\n",
    "width=320\n",
    "height=240"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入人脸和眼睛的HAAR 模型\n",
    "face_cascade = cv2.CascadeClassifier('./image/haarcascade_frontalface_default.xml')\n",
    "eye_cascade = cv2.CascadeClassifier('./image/haarcascade_eye.xml')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Video_display():\n",
    "    global pan\n",
    "    global tilt\n",
    "    while True: \n",
    "        frame = picamera.capture_array()\n",
    "        frame=cv2.flip(frame, 0)\n",
    "        gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)\n",
    "        faces = face_cascade.detectMultiScale(gray,1.3,5)\n",
    "\n",
    "        for(x,y,w,h) in faces:\n",
    "            cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),2)\n",
    "            Xcent = x + w/2\n",
    "            Ycent = y + h/2\n",
    "            errorPan  = Xcent - dispW/2 \n",
    "            errorTilt = Ycent - dispH/2\n",
    "            if abs(errorPan)>15:\n",
    "                pan=pan+errorPan/50\n",
    "            if abs(errorTilt)>15:\n",
    "                tilt=tilt+errorTilt/50\n",
    "            if pan > 180:\n",
    "                pan = 180\n",
    "                print(\"Pan out of Range\")\n",
    "            if pan < -90:\n",
    "                pan = -90\n",
    "                print(\"pan Out of Range\")\n",
    "            if tilt > 90:\n",
    "                tilt = 90\n",
    "                print(\"Pan out of Range\")\n",
    "            if tilt < -10:\n",
    "                tilt = -10\n",
    "                print(\"pan Out of Range\")\n",
    "\n",
    "            clbrobot.set_servo_angle(10,pan)\n",
    "            clbrobot.set_servo_angle(9, tilt)\n",
    "\n",
    "            roi_gray = gray[y:y+h, x:x+w]\n",
    "            roi_color = frame[y:y+h, x:x+w]        \n",
    "            eyes = eye_cascade.detectMultiScale(roi_gray)\n",
    "            for (ex,ey,ew,eh) in eyes:\n",
    "                cv2.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,255,0),2)          \n",
    "        frame_img.value = bgr8_to_jpeg(frame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# 开始线程\n",
    "t = threading.Thread(target=Video_display)\n",
    "t.setDaemon(True)\n",
    "t.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 如果摄像头云台方向不在正前方运行调整到正前方"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 结束线程\n",
    "stop_thread(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
